{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "id": "TCHw6F7Vw9Q_"
   },
   "outputs": [],
   "source": [
    "import time\n",
    "from random import random\n",
    "\n",
    "import numpy as np\n",
    "from pydrake.all import (\n",
    "    DiagramBuilder,\n",
    "    MeshcatVisualizer,\n",
    "    MultibodyPlant,\n",
    "    Parser,\n",
    "    RigidTransform,\n",
    "    RollPitchYaw,\n",
    "    RotationMatrix,\n",
    "    Simulator,\n",
    "    SolutionResult,\n",
    "    Solve,\n",
    "    StartMeshcat,\n",
    ")\n",
    "from pydrake.multibody import inverse_kinematics\n",
    "\n",
    "from manipulation import running_as_notebook\n",
    "from manipulation.exercises.trajectories.rrt_planner.robot import (\n",
    "    ConfigurationSpace,\n",
    "    Range,\n",
    ")\n",
    "from manipulation.exercises.trajectories.rrt_planner.rrt_planning import (\n",
    "    Problem,\n",
    ")\n",
    "from manipulation.meshcat_utils import AddMeshcatTriad\n",
    "from manipulation.scenarios import MakeManipulationStation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Start the visualizer.\n",
    "meshcat = StartMeshcat()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "class ManipulationStationSim:\n",
    "    def __init__(self, is_visualizing=False):\n",
    "        builder = DiagramBuilder()\n",
    "        self.station = builder.AddSystem(\n",
    "            MakeManipulationStation(\n",
    "                filename=\"package://manipulation/manipulation_station_with_cupboard.dmd.yaml\",\n",
    "                time_step=1e-3,\n",
    "            )\n",
    "        )\n",
    "        self.plant = self.station.GetSubsystemByName(\"plant\")\n",
    "        self.scene_graph = self.station.GetSubsystemByName(\"scene_graph\")\n",
    "        self.is_visualizing = is_visualizing\n",
    "\n",
    "        # scene graph query output port.\n",
    "        self.query_output_port = self.scene_graph.GetOutputPort(\"query\")\n",
    "\n",
    "        # meshcat visualizer\n",
    "        if is_visualizing:\n",
    "            self.viz = MeshcatVisualizer.AddToBuilder(\n",
    "                builder, self.station.GetOutputPort(\"query_object\"), meshcat\n",
    "            )\n",
    "\n",
    "        self.diagram = builder.Build()\n",
    "\n",
    "        # contexts\n",
    "        self.context_diagram = self.diagram.CreateDefaultContext()\n",
    "        self.context_station = self.diagram.GetSubsystemContext(\n",
    "            self.station, self.context_diagram\n",
    "        )\n",
    "        self.context_scene_graph = self.station.GetSubsystemContext(\n",
    "            self.scene_graph, self.context_station\n",
    "        )\n",
    "        self.context_plant = self.station.GetMutableSubsystemContext(\n",
    "            self.plant, self.context_station\n",
    "        )\n",
    "        # mark initial configuration\n",
    "        self.q0 = self.plant.GetPositions(\n",
    "            self.context_plant, self.plant.GetModelInstanceByName(\"iiwa\")\n",
    "        )\n",
    "        if is_visualizing:\n",
    "            self.DrawStation(self.q0, 0.1, -np.pi / 2, np.pi / 2)\n",
    "\n",
    "    def SetStationConfiguration(\n",
    "        self, q_iiwa, gripper_setpoint, left_door_angle, right_door_angle\n",
    "    ):\n",
    "        \"\"\"\n",
    "        :param q_iiwa: (7,) numpy array, joint angle of robots in radian.\n",
    "        :param gripper_setpoint: float, gripper opening distance in meters.\n",
    "        :param left_door_angle: float, left door hinge angle, \\in [0, pi/2].\n",
    "        :param right_door_angle: float, right door hinge angle, \\in [0, pi/2].\n",
    "        :return:\n",
    "        \"\"\"\n",
    "        self.plant.SetPositions(\n",
    "            self.context_plant,\n",
    "            self.plant.GetModelInstanceByName(\"iiwa\"),\n",
    "            q_iiwa,\n",
    "        )\n",
    "        self.plant.SetPositions(\n",
    "            self.context_plant,\n",
    "            self.plant.GetModelInstanceByName(\"wsg\"),\n",
    "            [-gripper_setpoint / 2, gripper_setpoint / 2],\n",
    "        )\n",
    "\n",
    "        # cabinet doors\n",
    "        if left_door_angle > 0:\n",
    "            left_door_angle *= -1\n",
    "        left_hinge_joint = self.plant.GetJointByName(\"left_door_hinge\")\n",
    "        left_hinge_joint.set_angle(\n",
    "            context=self.context_plant, angle=left_door_angle\n",
    "        )\n",
    "\n",
    "        right_hinge_joint = self.plant.GetJointByName(\"right_door_hinge\")\n",
    "        right_hinge_joint.set_angle(\n",
    "            context=self.context_plant, angle=right_door_angle\n",
    "        )\n",
    "\n",
    "    def DrawStation(self, q_iiwa, gripper_setpoint, q_door_left, q_door_right):\n",
    "        if not self.is_visualizing:\n",
    "            print(\"collision checker is not initialized with visualization.\")\n",
    "            return\n",
    "        self.SetStationConfiguration(\n",
    "            q_iiwa, gripper_setpoint, q_door_left, q_door_right\n",
    "        )\n",
    "        self.diagram.ForcedPublish(self.context_diagram)\n",
    "\n",
    "    def ExistsCollision(\n",
    "        self, q_iiwa, gripper_setpoint, q_door_left, q_door_right\n",
    "    ):\n",
    "        self.SetStationConfiguration(\n",
    "            q_iiwa, gripper_setpoint, q_door_left, q_door_right\n",
    "        )\n",
    "        query_object = self.query_output_port.Eval(self.context_scene_graph)\n",
    "        collision_paris = query_object.ComputePointPairPenetration()\n",
    "\n",
    "        return len(collision_paris) > 0\n",
    "\n",
    "\n",
    "class IiwaProblem(Problem):\n",
    "    def __init__(\n",
    "        self,\n",
    "        q_start: np.array,\n",
    "        q_goal: np.array,\n",
    "        gripper_setpoint: float,\n",
    "        left_door_angle: float,\n",
    "        right_door_angle: float,\n",
    "        is_visualizing=False,\n",
    "    ):\n",
    "        self.gripper_setpoint = gripper_setpoint\n",
    "        self.left_door_angle = left_door_angle\n",
    "        self.right_door_angle = right_door_angle\n",
    "        self.is_visualizing = is_visualizing\n",
    "\n",
    "        self.collision_checker = ManipulationStationSim(\n",
    "            is_visualizing=is_visualizing\n",
    "        )\n",
    "\n",
    "        # Construct configuration space for IIWA.\n",
    "        plant = self.collision_checker.plant\n",
    "        nq = 7\n",
    "        joint_limits = np.zeros((nq, 2))\n",
    "        for i in range(nq):\n",
    "            joint = plant.GetJointByName(\"iiwa_joint_%i\" % (i + 1))\n",
    "            joint_limits[i, 0] = joint.position_lower_limits()\n",
    "            joint_limits[i, 1] = joint.position_upper_limits()\n",
    "\n",
    "        range_list = []\n",
    "        for joint_limit in joint_limits:\n",
    "            range_list.append(Range(joint_limit[0], joint_limit[1]))\n",
    "\n",
    "        def l2_distance(q: tuple):\n",
    "            sum = 0\n",
    "            for q_i in q:\n",
    "                sum += q_i**2\n",
    "            return np.sqrt(sum)\n",
    "\n",
    "        max_steps = nq * [np.pi / 180 * 2]  # three degrees\n",
    "        cspace_iiwa = ConfigurationSpace(range_list, l2_distance, max_steps)\n",
    "\n",
    "        # Call base class constructor.\n",
    "        Problem.__init__(\n",
    "            self,\n",
    "            x=10,  # not used.\n",
    "            y=10,  # not used.\n",
    "            robot=None,  # not used.\n",
    "            obstacles=None,  # not used.\n",
    "            start=tuple(q_start),\n",
    "            goal=tuple(q_goal),\n",
    "            cspace=cspace_iiwa,\n",
    "        )\n",
    "\n",
    "    def collide(self, configuration):\n",
    "        q = np.array(configuration)\n",
    "        return self.collision_checker.ExistsCollision(\n",
    "            q,\n",
    "            self.gripper_setpoint,\n",
    "            self.left_door_angle,\n",
    "            self.right_door_angle,\n",
    "        )\n",
    "\n",
    "    def visualize_path(self, path):\n",
    "        if path is not None:\n",
    "            # show path in meshcat\n",
    "            for q in path:\n",
    "                q = np.array(q)\n",
    "                self.collision_checker.DrawStation(\n",
    "                    q,\n",
    "                    self.gripper_setpoint,\n",
    "                    self.left_door_angle,\n",
    "                    self.right_door_angle,\n",
    "                )\n",
    "                if running_as_notebook:\n",
    "                    time.sleep(0.2)\n",
    "\n",
    "\n",
    "class IKSolver(object):\n",
    "    def __init__(self):\n",
    "        ## setup controller plant\n",
    "        plant_iiwa = MultibodyPlant(0.0)\n",
    "        iiwa_file = \"package://drake/manipulation/models/iiwa_description/iiwa7/iiwa7_no_collision.sdf\"\n",
    "        iiwa = Parser(plant_iiwa).AddModelsFromUrl(iiwa_file)[0]\n",
    "        # Define frames\n",
    "        world_frame = plant_iiwa.world_frame()\n",
    "        L0 = plant_iiwa.GetFrameByName(\"iiwa_link_0\")\n",
    "        l7_frame = plant_iiwa.GetFrameByName(\"iiwa_link_7\")\n",
    "        plant_iiwa.WeldFrames(world_frame, L0)\n",
    "        plant_iiwa.Finalize()\n",
    "        plant_context = plant_iiwa.CreateDefaultContext()\n",
    "\n",
    "        # gripper in link 7 frame\n",
    "        X_L7G = RigidTransform(\n",
    "            rpy=RollPitchYaw([np.pi / 2, 0, np.pi / 2]), p=[0, 0, 0.114]\n",
    "        )\n",
    "        world_frame = plant_iiwa.world_frame()\n",
    "\n",
    "        self.world_frame = world_frame\n",
    "        self.l7_frame = l7_frame\n",
    "        self.plant_iiwa = plant_iiwa\n",
    "        self.plant_context = plant_context\n",
    "        self.X_L7G = X_L7G\n",
    "\n",
    "    def solve(self, X_WT, q_guess=None, theta_bound=0.01, position_bound=0.01):\n",
    "        \"\"\"\n",
    "        plant: a mini plant only consists of iiwa arm with no gripper attached\n",
    "        X_WT: transform of target frame in world frame\n",
    "        q_guess: a guess on the joint state sol\n",
    "        \"\"\"\n",
    "        plant = self.plant_iiwa\n",
    "        l7_frame = self.l7_frame\n",
    "        X_L7G = self.X_L7G\n",
    "        world_frame = self.world_frame\n",
    "\n",
    "        R_WT = X_WT.rotation()\n",
    "        p_WT = X_WT.translation()\n",
    "\n",
    "        if q_guess is None:\n",
    "            q_guess = np.zeros(7)\n",
    "\n",
    "        ik_instance = inverse_kinematics.InverseKinematics(plant)\n",
    "        # align frame A to frame B\n",
    "        ik_instance.AddOrientationConstraint(\n",
    "            frameAbar=l7_frame,\n",
    "            R_AbarA=X_L7G.rotation(),\n",
    "            #   R_AbarA=RotationMatrix(), # for link 7\n",
    "            frameBbar=world_frame,\n",
    "            R_BbarB=R_WT,\n",
    "            theta_bound=position_bound,\n",
    "        )\n",
    "        # align point Q in frame B to the bounding box in frame A\n",
    "        ik_instance.AddPositionConstraint(\n",
    "            frameB=l7_frame,\n",
    "            p_BQ=X_L7G.translation(),\n",
    "            # p_BQ=[0,0,0], # for link 7\n",
    "            frameA=world_frame,\n",
    "            p_AQ_lower=p_WT - position_bound,\n",
    "            p_AQ_upper=p_WT + position_bound,\n",
    "        )\n",
    "        prog = ik_instance.prog()\n",
    "        prog.SetInitialGuess(ik_instance.q(), q_guess)\n",
    "        result = Solve(prog)\n",
    "        if result.get_solution_result() != SolutionResult.kSolutionFound:\n",
    "            return result.GetSolution(ik_instance.q()), False\n",
    "        return result.GetSolution(ik_instance.q()), True\n",
    "\n",
    "\n",
    "class TreeNode:\n",
    "    def __init__(self, value, parent=None):\n",
    "        self.value = value  # tuple of floats representing a configuration\n",
    "        self.parent = parent  # another TreeNode\n",
    "        self.children = []  # list of TreeNodes\n",
    "\n",
    "\n",
    "class RRT:\n",
    "    \"\"\"\n",
    "    RRT Tree.\n",
    "    \"\"\"\n",
    "\n",
    "    def __init__(self, root: TreeNode, cspace: ConfigurationSpace):\n",
    "        self.root = root  # root TreeNode\n",
    "        self.cspace = cspace  # robot.ConfigurationSpace\n",
    "        self.size = 1  # int length of path\n",
    "        self.max_recursion = 1000  # int length of longest possible path\n",
    "\n",
    "    def add_configuration(self, parent_node, child_value):\n",
    "        child_node = TreeNode(child_value, parent_node)\n",
    "        parent_node.children.append(child_node)\n",
    "        self.size += 1\n",
    "        return child_node\n",
    "\n",
    "    # Brute force nearest, handles general distance functions\n",
    "    def nearest(self, configuration):\n",
    "        \"\"\"\n",
    "        Finds the nearest node by distance to configuration in the\n",
    "             configuration space.\n",
    "\n",
    "        Args:\n",
    "            configuration: tuple of floats representing a configuration of a\n",
    "                robot\n",
    "\n",
    "        Returns:\n",
    "            closest: TreeNode. the closest node in the configuration space\n",
    "                to configuration\n",
    "            distance: float. distance from configuration to closest\n",
    "        \"\"\"\n",
    "        assert self.cspace.valid_configuration(configuration)\n",
    "\n",
    "        def recur(node, depth=0):\n",
    "            closest, distance = node, self.cspace.distance(\n",
    "                node.value, configuration\n",
    "            )\n",
    "            if depth < self.max_recursion:\n",
    "                for child in node.children:\n",
    "                    (child_closest, child_distance) = recur(child, depth + 1)\n",
    "                    if child_distance < distance:\n",
    "                        closest = child_closest\n",
    "                        child_distance = child_distance\n",
    "            return closest, distance\n",
    "\n",
    "        return recur(self.root)[0]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "wJxE0ZSgxDFl"
   },
   "source": [
    "# RRT Motion Planning\n",
    "\n",
    "In the lectures on motion planning, you are introduced to optimization-based motion planning and sampling-based motion planning. In this exercise, you will first implement the famous Rapidly-exploring Random Tree (RRT) algorithm. Next, you will reflect on the properties of the RRT algorithm. A 2D visualization of the RRT algorithm is shown below. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "id": "ssfgBwB3x2yc"
   },
   "outputs": [],
   "source": [
    "from IPython.display import Image\n",
    "\n",
    "Image(\n",
    "    url=\"https://upload.wikimedia.org/wikipedia/commons/thumb/6/62/Rapidly-exploring_Random_Tree_%28RRT%29_500x373.gif/450px-Rapidly-exploring_Random_Tree_%28RRT%29_500x373.gif\"\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "YkAeqwCHydaP"
   },
   "source": [
    "Let's first generate a problem instance. Let's use the default initial joint state as our starting configuration $q_{start}$. Let's use a pre-defined frame in 3D world as our goal pose. The frame of the goal pose can be viewed in the meshcat visualizer below. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "id": "IRzTg8bdw9RF"
   },
   "outputs": [],
   "source": [
    "env = ManipulationStationSim(True)\n",
    "q_start = env.q0\n",
    "R_WG = RotationMatrix(np.array([[0, 1, 0], [1, 0, 0], [0, 0, -1]]).T)\n",
    "T_WG_goal = RigidTransform(\n",
    "    p=np.array([4.69565839e-01, 2.95894043e-16, 0.65]), R=R_WG\n",
    ")\n",
    "AddMeshcatTriad(meshcat, \"goal pose\", X_PT=T_WG_goal, opacity=0.5)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "ZHj1mtAuzP1F"
   },
   "source": [
    "The joint states of the goal pose can be computed via inverse kinematics."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "id": "0v_4tS_xw9RL"
   },
   "outputs": [],
   "source": [
    "ik_solver = IKSolver()\n",
    "q_goal, optimal = ik_solver.solve(T_WG_goal, q_guess=q_start)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "_iPAypZPzgQd"
   },
   "source": [
    "Given the start and goal states, we now have sufficient information to formulate the pathfinding problem. We use `IiwaProblem` class to store all relevant information about the pathfinding problem. For this exercise, you don't have to know the details of this class."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "id": "exkFjk8jw9RP"
   },
   "outputs": [],
   "source": [
    "gripper_setpoint = 0.1\n",
    "door_angle = np.pi / 2 - 0.001\n",
    "left_door_angle = -np.pi / 2\n",
    "right_door_angle = np.pi / 2\n",
    "\n",
    "iiwa_problem = IiwaProblem(\n",
    "    q_start=q_start,\n",
    "    q_goal=q_goal,\n",
    "    gripper_setpoint=gripper_setpoint,\n",
    "    left_door_angle=left_door_angle,\n",
    "    right_door_angle=right_door_angle,\n",
    "    is_visualizing=True,\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "njvk8Yim0AF7"
   },
   "source": [
    "# RRT Algorithm\n",
    "\n",
    "An RRT grows a tree rooted at the starting configuration by using random samples from the search space. As each sample is drawn, a connection is attempted between it and the nearest state in the tree. If the connection is feasible (passes entirely through free space and obeys any constraints), this results in the addition of the new state to the tree.\n",
    "\n",
    "With uniform sampling of the search space, the probability of expanding an existing state is proportional to the size of its Voronoi region. As the largest Voronoi regions belong to the states on the frontier of the search, this means that the tree preferentially expands towards large unsearched areas.\n",
    "\n",
    "However, it may be useful sometimes to bias our exploration towards the goal. In that case, one can artificially set a probability to use the value of the goal as the next sample. \n",
    "\n",
    "The pseudocode of the RRT algorithm is shown below."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "GDLn7bfYw9RT"
   },
   "source": [
    "  **Algorithm RRT**\n",
    "    \n",
    "      Input: q_start, q_goal, max_interation, prob_sample_goal\n",
    "      Output: path\n",
    "\n",
    "      G.init(q_start)\n",
    "      for k = 1 to max_interation:\n",
    "        q_sample \u2190 Generate Random Configuration\n",
    "        random number \u2190 random()\n",
    "        if random_number < prob_sample_goal:\n",
    "            q_sample \u2190 q_goal\n",
    "        n_near \u2190 Find the nearest node in the tree(q_sample)\n",
    "        (q_1, q_2, ... q_N) \u2190 Find intermediate q's from n_near to q_sample\n",
    "        \n",
    "        // iteratively add the new nodes to the tree to form a new edge\n",
    "        last_node \u2190 n_near\n",
    "        for n = 1 to N:\n",
    "            last_node \u2190 Grow RRT tree (parent_node, q_{n}) \n",
    "        \n",
    "        if last node reaches the goal:\n",
    "            path \u2190 backup the path recursively\n",
    "            return path\n",
    "        \n",
    "      return None"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "drJRhESa3VIM"
   },
   "source": [
    "Implementing RRT from scratch can be very time-consuming. Below, we have provided you the important features you will need to implement the RRT algorithm. Note that in `RRT_tools`, a robot configuration is referred to as $q$, whereas a node in the RRT tree is referred to as a node. One can access the configuration of a node by \n",
    "```\n",
    "q_sample = node.value\n",
    "```"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "id": "lrYX6hxsw9RU"
   },
   "outputs": [],
   "source": [
    "class RRT_tools:\n",
    "    def __init__(self, problem):\n",
    "        # rrt is a tree\n",
    "        self.rrt_tree = RRT(TreeNode(problem.start), problem.cspace)\n",
    "        problem.rrts = [self.rrt_tree]\n",
    "        self.problem = problem\n",
    "\n",
    "    def find_nearest_node_in_RRT_graph(self, q_sample):\n",
    "        nearest_node = self.rrt_tree.nearest(q_sample)\n",
    "        return nearest_node\n",
    "\n",
    "    def sample_node_in_configuration_space(self):\n",
    "        q_sample = self.problem.cspace.sample()\n",
    "        return q_sample\n",
    "\n",
    "    def calc_intermediate_qs_wo_collision(self, q_start, q_end):\n",
    "        \"\"\"create more samples by linear interpolation from q_start\n",
    "        to q_end. Return all samples that are not in collision\n",
    "\n",
    "        Example interpolated path:\n",
    "        q_start, qa, qb, (Obstacle), qc , q_end\n",
    "        returns >>> q_start, qa, qb\n",
    "        \"\"\"\n",
    "        return self.problem.safe_path(q_start, q_end)\n",
    "\n",
    "    def grow_rrt_tree(self, parent_node, q_sample):\n",
    "        \"\"\"\n",
    "        add q_sample to the rrt tree as a child of the parent node\n",
    "        returns the rrt tree node generated from q_sample\n",
    "        \"\"\"\n",
    "        child_node = self.rrt_tree.add_configuration(parent_node, q_sample)\n",
    "        return child_node\n",
    "\n",
    "    def node_reaches_goal(self, node):\n",
    "        return node.value == self.problem.goal\n",
    "\n",
    "    def backup_path_from_node(self, node):\n",
    "        path = [node.value]\n",
    "        while node.parent is not None:\n",
    "            node = node.parent\n",
    "            path.append(node.value)\n",
    "        path.reverse()\n",
    "        return path"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "lsRzky8W4nfb"
   },
   "source": [
    "## Implement RRT\n",
    "\n",
    "**(a) Implement the RRT algorithm below. You may find it significantly easier to use the `RRT_tools`.** \n",
    "In your implementation, you may plan in either configuration space or task space. The provided `RRT_tools` is only for planning in the configuration space. Your implementation will be graded on whether the last node of the path has reached the goal."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "id": "YcvgX_B9w9RY"
   },
   "outputs": [],
   "source": [
    "def rrt_planning(problem, max_iterations=1000, prob_sample_q_goal=0.05):\n",
    "    \"\"\"\n",
    "    Input:\n",
    "        problem (IiwaProblem): instance of a utility class\n",
    "        max_iterations: the maximum number of samples to be collected\n",
    "        prob_sample_q_goal: the probability of sampling q_goal\n",
    "\n",
    "    Output:\n",
    "        path (list): [q_start, ...., q_goal].\n",
    "                    Note q's are configurations, not RRT nodes\n",
    "    \"\"\"\n",
    "    rrt_tools = RRT_tools(iiwa_problem)\n",
    "    q_goal = problem.goal\n",
    "    q_start = problem.start\n",
    "\n",
    "    return None"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "id": "I3ZOEcE9w9Rh"
   },
   "outputs": [],
   "source": [
    "path = rrt_planning(iiwa_problem, 600, 0.05)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "d852c3Oa5IVF"
   },
   "source": [
    "You may step through the waypoints of the planned path below."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "id": "Gd7CC-3Cw9Rm"
   },
   "outputs": [],
   "source": [
    "iiwa_problem.visualize_path(path)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "fMhnEoWWw9Rp"
   },
   "source": [
    "**Answer the following question regarding the properties of the RRT algorithm**\n",
    "\n",
    "(b) Consider the case where we let our RRT algorithm run forever, i..e max_iterations is set to $\\infty$. If there is no path to the goal, will RRT warn you? If there is a path to the goal, will RRT eventually find that path? Explain your reasoning for both cases. \n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "MwE8yNg58VQN"
   },
   "source": [
    "## How will this notebook be Graded?\n",
    "\n",
    "If you are enrolled in the class, this notebook will be graded using [Gradescope](www.gradescope.com). You should have gotten the enrollement code on our announcement in Piazza. \n",
    "\n",
    "For submission of this assignment, you must do two things. \n",
    "- Download and submit the notebook `rrt_planning.ipynb` to Gradescope's notebook submission section, along with your notebook for the other problems.\n",
    "- Write down your answers to 6.2.b in your PDF submission to Gradescope. \n",
    "\n",
    "We will evaluate the local functions in the notebook to see if the function behaves as we have expected. For this exercise, the rubric is as follows:\n",
    "- [5 pts] Correct Implementation of `rrt_planning` method.\n",
    "- [3 pts] Reasonable answers and explanations for part (b) "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "id": "XjQh6E-r_CNx"
   },
   "outputs": [],
   "source": [
    "from manipulation.exercises.trajectories.test_rrt_planning import TestRRT\n",
    "from manipulation.exercises.grader import Grader\n",
    "\n",
    "Grader.grade_output([TestRRT], [locals()], \"results.json\")\n",
    "Grader.print_test_results(\"results.json\")"
   ]
  }
 ],
 "metadata": {
  "celltoolbar": "Tags",
  "colab": {
   "collapsed_sections": [
    "GC4aZuODw9Q9"
   ],
   "name": "rrt_planning.ipynb",
   "provenance": []
  },
  "kernelspec": {
   "display_name": "Python 3.8.10 64-bit",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.10"
  },
  "vscode": {
   "interpreter": {
    "hash": "e7370f93d1d0cde622a1f8e1c04877d8463912d04d973331ad4851f04de6915a"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}